import serial
import time
import json
from .base_vehicle_controller import BaseVehicleController
from .controller_registry import ControllerRegistry
from .config import CONTROLLER_CONFIG

@ControllerRegistry.register("DifferentialDrive")
class DifferentialDriveController(BaseVehicleController):
    def __init__(self):
        super().__init__()
        self.last_cmd_time = 0
        self.max_speed = CONTROLLER_CONFIG['max_speed']
        self.controller_type = "DifferentialDrive"
        
    def send_command(self, command):
        try:
            # 直接使用命令字典，无需JSON解析
            if 'left_speed' in command and 'right_speed' in command:
                left_speed = int(command['left_speed'])
                right_speed = int(command['right_speed'])
                
                # 确保速度在允许范围内
                left_speed = max(-self.max_speed, min(self.max_speed, left_speed))
                right_speed = max(-self.max_speed, min(self.max_speed, right_speed))
                
                # 构建要发送到Arduino的命令
                arduino_cmd = f"L{left_speed}R{right_speed}\n"
                
                if hasattr(self, 'arduino') and isinstance(self.arduino, serial.Serial):
                    self.arduino.write(arduino_cmd.encode('utf-8'))
                    self.last_cmd_time = time.time()
                    return True
                else:
                    print("Serial connection not established")
                    return False
            else:
                print("Invalid command format. Expected 'left_speed' and 'right_speed'.")
                return False
        except Exception as e:
            print(f"An error occurred while sending command: {str(e)}")
            return False
